/**
 ********************************************************************
 * @file    test_positioning.c
 * @brief
 *
 * @copyright (c) 2021 DJI. All rights reserved.
 *
 * All information contained herein is, and remains, the property of DJI.
 * The intellectual and technical concepts contained herein are proprietary
 * to DJI and may be covered by U.S. and foreign patents, patents in process,
 * and protected by trade secret or copyright law.  Dissemination of this
 * information, including but not limited to data and other proprietary
 * material(s) incorporated within the information, in any form, is strictly
 * prohibited without the express written consent of DJI.
 *
 * If you receive this source code without DJI’s authorization, you may not
 * further disseminate the information, and you must immediately remove the
 * source code and notify DJI of its removal. DJI reserves the right to pursue
 * legal actions against you for any loss(es) or damage(s) caused by your
 * failure to do so.
 *
 *********************************************************************
 */


/* Includes ------------------------------------------------------------------*/
#include <fc_subscription/test_fc_subscription.h>
#include "test_positioning.h"
#include "dji_positioning.h"
#include "dji_network_rtk.h"
#include "dji_logger.h"
#include "utils/util_misc.h"
#include "dji_platform.h"
#include "time_sync/test_time_sync.h"

#ifdef SYSTEM_ARCH_LINUX

#include "time.h"

#endif

/* Private constants ---------------------------------------------------------*/
#define POSITIONING_TASK_FREQ                     (0.1)
#define POSITIONING_TASK_STACK_SIZE               (3 * 1024)
#define TEST_RTCM_FILE_PATH_STR_MAX_SIZE          (64)

#define DJI_TEST_POSITIONING_EVENT_COUNT          (1)
#define DJI_TEST_TIME_INTERVAL_AMONG_EVENTS_US    (200000)

/* Private types -------------------------------------------------------------*/

/* Private functions declaration ---------------------------------------------*/
static void *DjiTest_PositioningTask(void *arg);
static T_DjiReturnCode DjiTest_ReceiveRtkOnAircraftRtcmDataCallback(uint8_t index, const uint8_t *data,
                                                                    uint16_t dataLen);
static T_DjiReturnCode DjiTest_ReceiveRtkBaseStationRtcmDataCallback(uint8_t index, const uint8_t *data,
                                                                     uint16_t dataLen);

/* Private variables ---------------------------------------------------------*/
static T_DjiTaskHandle s_userPositioningThread;
static int32_t s_eventIndex = 0;
#ifdef SYSTEM_ARCH_LINUX
static char s_rtkOnAircraftRtcmFilePath[TEST_RTCM_FILE_PATH_STR_MAX_SIZE];
static char s_rtkBaseStationRtcmFilePath[TEST_RTCM_FILE_PATH_STR_MAX_SIZE];
#endif

/* Exported functions definition ---------------------------------------------*/
T_DjiReturnCode DjiTest_PositioningStartService(void)
{
    T_DjiReturnCode djiStat;
    T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();

    djiStat = DjiPositioning_Init();
    if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
        USER_LOG_ERROR("positioning module init error.");
        return djiStat;
    }

    DjiPositioning_SetTaskIndex(0);

#ifndef SYSTEM_ARCH_LINUX
    if (osalHandler->TaskCreate("user_positioning_task", DjiTest_PositioningTask,
                                POSITIONING_TASK_STACK_SIZE, NULL, &s_userPositioningThread) !=
        DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
        USER_LOG_ERROR("user ps_extensionPortSampleIndexositioning task create error.");
        return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
    }
#else
    time_t currentTime = time(NULL);
    struct tm *localTime = NULL;

    localTime = localtime(&currentTime);
    sprintf(s_rtkOnAircraftRtcmFilePath, "rtk_on_aircraft_%04d%02d%02d_%02d-%02d-%02d.rtcm",
            localTime->tm_year + 1900, localTime->tm_mon + 1, localTime->tm_mday,
            localTime->tm_hour, localTime->tm_min, localTime->tm_sec);

    localTime = localtime(&currentTime);
    sprintf(s_rtkBaseStationRtcmFilePath, "rtk_base_station_%04d%02d%02d_%02d-%02d-%02d.rtcm",
            localTime->tm_year + 1900, localTime->tm_mon + 1, localTime->tm_mday,
            localTime->tm_hour, localTime->tm_min, localTime->tm_sec);
#endif
    djiStat = DjiPositioning_RegReceiveRtcmDataCallback(DJI_POSITIONING_RTCM_DATA_TYPE_RTK_BASE_STATION,
                                                        DjiTest_ReceiveRtkBaseStationRtcmDataCallback);
    if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
        USER_LOG_ERROR("Register receive rtk base station callback error.");
        return djiStat;
    }

    djiStat = DjiPositioning_RegReceiveRtcmDataCallback(DJI_POSITIONING_RTCM_DATA_TYPE_RTK_ON_AIRCRAFT,
                                                        DjiTest_ReceiveRtkOnAircraftRtcmDataCallback);
    if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
        USER_LOG_ERROR("Register receive rtk on aircraft callback error.");
        return djiStat;
    }

    return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

static void DjiTest_ReceiveNetworkRtkStateCallback(E_DjiNetworkRtkOnboardState state)
{
    USER_LOG_INFO("Network rtk state: %d", state);
}

T_DjiReturnCode DjiTest_NetworkRtkOnBoardService(E_DjiTestNetworkRtkCtrl ctrl)
{
    T_DjiReturnCode djiStat;
    USER_LOG_INFO("Network rtk service %s..", ctrl == DJI_TEST_NETWORK_RTK_START ? "start":"stop");
    switch (ctrl)
    {
    case DJI_TEST_NETWORK_RTK_START:
        djiStat = DjiNetworkRtk_RegReceiveNetworkRtkStateCallback(DjiTest_ReceiveNetworkRtkStateCallback);
        if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
        {
            USER_LOG_ERROR("Register receive network rtk state callback error.");
            return djiStat;
        }
        /* example account */
        T_DjiNetworkRtkServiceConfig config = {
            .account = "12345",
            .password = "12345",
            .host = "12345",
            .port = "123",
            .mountPoint = "234",
        };
        djiStat = DjiNetworkRtk_StartService(&config);
        if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
        {
            USER_LOG_ERROR("start network rtk on aircraft error.");
            return djiStat;
        }
        break;
    case DJI_TEST_NETWORK_RTK_STOP:
        djiStat = DjiNetworkRtk_StopService();
        if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
        {
            USER_LOG_ERROR("stop network rtk on aircraft error.");
            return djiStat;
        }
        break;
    }
    return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

/* Private functions definition-----------------------------------------------*/
#ifndef __CC_ARM
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wmissing-noreturn"
#pragma GCC diagnostic ignored "-Wreturn-type"
#endif

static void *DjiTest_PositioningTask(void *arg)
{
    int32_t i = 0;
    T_DjiReturnCode djiStat;
    uint64_t ppsNewestTriggerTimeUs = 0;
    T_DjiPositioningEventInfo eventInfo[DJI_TEST_POSITIONING_EVENT_COUNT] = {0};
    T_DjiPositioningPositionInfo positionInfo[DJI_TEST_POSITIONING_EVENT_COUNT] = {0};
    T_DjiTimeSyncAircraftTime aircraftTime = {0};
    T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
    uint8_t totalSatelliteNumber = 0;

    USER_UTIL_UNUSED(arg);

    while (1) {
        osalHandler->TaskSleepMs(1000 / POSITIONING_TASK_FREQ);

        djiStat = DjiTest_FcSubscriptionGetTotalSatelliteNumber(&totalSatelliteNumber);
        if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
            USER_LOG_ERROR("get total satellite number error: 0x%08llX.", djiStat);
            continue;
        }

        djiStat = DjiTest_TimeSyncGetNewestPpsTriggerLocalTimeUs(&ppsNewestTriggerTimeUs);
        if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
            USER_LOG_ERROR("get newest pps trigger time error: 0x%08llX.", djiStat);
            continue;
        }

        for (i = 0; i < DJI_TEST_POSITIONING_EVENT_COUNT; ++i) {
            eventInfo[i].eventSetIndex = s_eventIndex;
            eventInfo[i].targetPointIndex = i;

            djiStat = DjiTimeSync_TransferToAircraftTime(
                ppsNewestTriggerTimeUs - 1000000 - i * DJI_TEST_TIME_INTERVAL_AMONG_EVENTS_US, &aircraftTime);
            if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
                USER_LOG_ERROR("transfer to aircraft time error: 0x%08llX.", djiStat);
                continue;
            }

            eventInfo[i].eventTime = aircraftTime;
        }

        djiStat = DjiPositioning_GetPositionInformationSync(DJI_TEST_POSITIONING_EVENT_COUNT, eventInfo, positionInfo);
        if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
            USER_LOG_ERROR("get position information error.");
            continue;
        }

        USER_LOG_INFO("request position of target points success.");
        USER_LOG_INFO("detail position information:");
        for (i = 0; i < DJI_TEST_POSITIONING_EVENT_COUNT; ++i) {
            USER_LOG_INFO("position solution property: %d.", positionInfo[i].positionSolutionProperty);
            USER_LOG_INFO("pitchAttitudeAngle: %d\trollAttitudeAngle: %d\tyawAttitudeAngle: %d",
                           positionInfo[i].uavAttitude.pitch, positionInfo[i].uavAttitude.roll,
                           positionInfo[i].uavAttitude.yaw);
            USER_LOG_INFO("northPositionOffset: %d\tearthPositionOffset: %d\tdownPositionOffset: %d",
                           positionInfo[i].offsetBetweenMainAntennaAndTargetPoint.x,
                           positionInfo[i].offsetBetweenMainAntennaAndTargetPoint.y,
                           positionInfo[i].offsetBetweenMainAntennaAndTargetPoint.z);
            USER_LOG_INFO("longitude: %.8f\tlatitude: %.8f\theight: %.8f",
                           positionInfo[i].targetPointPosition.longitude,
                           positionInfo[i].targetPointPosition.latitude,
                           positionInfo[i].targetPointPosition.height);
            USER_LOG_INFO(
                "longStandardDeviation: %.8f\tlatStandardDeviation: %.8f\thgtStandardDeviation: %.8f",
                positionInfo[i].targetPointPositionStandardDeviation.longitude,
                positionInfo[i].targetPointPositionStandardDeviation.latitude,
                positionInfo[i].targetPointPositionStandardDeviation.height);
        }

        s_eventIndex++;
    }
}

#ifndef __CC_ARM
#pragma GCC diagnostic pop
#endif

#ifdef SYSTEM_ARCH_LINUX
static int32_t DjiTest_SaveRtcmData(char *filePath, const uint8_t *data, uint32_t len)
{
    FILE *fp = NULL;
    size_t size;

    fp = fopen(filePath, "ab+");
    if (fp == NULL) {
        printf("fopen failed!\n");
        return -1;
    }

    size = fwrite(data, 1, len, fp);
    if (size != len) {
        fclose(fp);
        return -1;
    }

    fflush(fp);
    fclose(fp);
    return 0;
}
#endif

static T_DjiReturnCode DjiTest_ReceiveRtkOnAircraftRtcmDataCallback(uint8_t index, const uint8_t *data,
                                                                    uint16_t dataLen)
{
    USER_LOG_INFO("Receive rtcm data from rtk on aircraft, index: %d, len: %d", index, dataLen);

#ifdef SYSTEM_ARCH_LINUX
    DjiTest_SaveRtcmData(s_rtkOnAircraftRtcmFilePath, data, dataLen);
#endif
    return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

static T_DjiReturnCode DjiTest_ReceiveRtkBaseStationRtcmDataCallback(uint8_t index, const uint8_t *data,
                                                                     uint16_t dataLen)
{
    USER_LOG_INFO("Receive rtcm data from rtk base station, index: %d, len: %d", index, dataLen);

#ifdef SYSTEM_ARCH_LINUX
    DjiTest_SaveRtcmData(s_rtkBaseStationRtcmFilePath, data, dataLen);
#endif
    return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}


/****************** (C) COPYRIGHT DJI Innovations *****END OF FILE****/
